//
// Created by ZhaoXiaoFei on 2022/8/19.
//

#include "model/imuPredict.hpp"

ImuPredict::ImuPredict(const double acc_noise, const double gyr_noise,
           const double acc_bias_noise, const double gyr_bias_noise, const Eigen::Vector3d G){
    acc_noise_ = acc_noise;
    gyr_noise_ = gyr_noise;
    acc_bias_noise_ = acc_bias_noise;
    gyr_bias_noise_ = gyr_bias_noise;
    G_ = G;
}

void ImuPredict::predict(const ImuData imu_data, State* state){
//        if(imu_data.timestamp < state->imuData.timestamp){
//            return;
//        }
    State* lastState = state;
    double dt = imu_data.timestamp - lastState->imuData.timestamp;
    //更新名义值
    Eigen::Vector3d un_gyr = 0.5 * (lastState->imuData.gyr + imu_data.gyr) - lastState->gyr_bias_;
    Eigen::Vector3d d_theta = un_gyr * dt;
    Eigen::AngleAxisd d_rot(d_theta.norm(), d_theta.normalized());
    state->pose_.block<3,3>(0,0) = lastState->pose_.block<3,3>(0,0) * d_rot.toRotationMatrix();

    Eigen::Vector3d un_acc = 0.5 * (lastState->pose_.block<3,3>(0,0) * (lastState->imuData.acc - lastState->acc_bias_) +
                                    state->pose_.block<3,3>(0,0) * (imu_data.acc - lastState->acc_bias_));
    //b 系下速度均值
    Eigen::Vector3d un_acc_b = 0.5 * ((lastState->imuData.acc - lastState->acc_bias_) + (imu_data.acc - lastState->acc_bias_));

    state->pose_.block<3,1>(0,3) = lastState->pose_.block<3,1>(0,3) + dt * lastState->vel_ +
                                   0.5 * dt * dt * (un_acc + G_);
    state->vel_ = lastState->vel_ + dt * (un_acc + G_);

    Eigen::Matrix<double,15,15> Ft = Eigen::Matrix<double,15,15>::Zero();
    Ft.block<3, 3>(0,3) = Eigen::Matrix3d::Identity();
    Ft.block<3, 3>(3, 6) = -state->pose_.block<3,3>(0,0) *  Sophus::SO3d::hat(un_acc_b).matrix();
    Ft.block<3, 3>(3, 9) = -state->pose_.block<3,3>(0,0);
    Ft.block<3, 3>(6, 6) =  -Sophus::SO3d::hat(un_gyr).matrix();
    Ft.block<3, 3>(6, 12) =  -Eigen::Matrix3d::Identity();
    // b. set process equation for delta ori:
    Eigen::Matrix<double,15,12> Bt = Eigen::Matrix<double,15,12>::Zero();
    Bt.block<3, 3>(3, 0) = state->pose_.block<3,3>(0,0);
    Bt.block<3, 3>(6, 3) = Eigen::Matrix3d::Identity();
    Bt.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity();
    Bt.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity();

    Eigen::Matrix<double,12,12> Q = Eigen::Matrix<double,12,12>::Zero();
    Q.block<3,3>(0,0) = Eigen::Matrix3d::Identity() * acc_noise_ * dt * dt;
    Q.block<3,3>(3,3) = Eigen::Matrix3d::Identity() * gyr_noise_ * dt * dt;
    Q.block<3,3>(6,6) = Eigen::Matrix3d::Identity() * acc_bias_noise_ * dt;
    Q.block<3,3>(9,9) = Eigen::Matrix3d::Identity() * gyr_bias_noise_ * dt;

    Eigen::Matrix<double,15,15> Fk = Eigen::Matrix<double,15,15>::Zero();
    Eigen::Matrix<double,15,12> Bk = Eigen::Matrix<double,15,12>::Zero();
    Fk = Eigen::Matrix<double,15,15>::Identity() + Ft * dt;
    Bk = Bt;
    state->X_ = Fk * lastState->X_;
    state->P_ = Fk * lastState->P_ * Fk.transpose() + Bk * Q * Bk.transpose();

//        LOG(INFO) << "-------------------"<< std::endl;
//        LOG(INFO) << state->P_ << std::endl;

    state->timestamp = imu_data.timestamp;
    state->imuData = imu_data;
//        LOG(INFO) << state->pose_.block<3, 1>(0, 3).transpose() << std::endl;
}